/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;

/**
 *
 * @author hpadmin
 */
public class Manipulator {
    protected DigitalInput extendedSwitch;
    protected DigitalInput retractedSwitch;
    protected Jaguar motor;
    protected double trim;
    protected boolean retractedSwitchEnabled = true;
    protected boolean extendedSwitchEnabled = true;

    protected Manipulator(int extendedSwitchPort, int retractedSwitchPort, int motorPort){
        extendedSwitch = new DigitalInput(extendedSwitchPort);
        retractedSwitch = new DigitalInput(retractedSwitchPort);
        motor = new Jaguar(motorPort);

        trim = 0;
    }


    public void setVelocity(double vel){
        if((vel > 0) && (extendedSwitch.get() == true) && extendedSwitchEnabled ){
            vel = 0;
        }
        else if((vel < 0) && (retractedSwitch.get() == true) && retractedSwitchEnabled )
        {
            vel = 0;
        }
        motor.set(vel + trim);

        //System.out.println("Velocity set to: " + vel + "LS " + extendedSwitch.get() + " " + retractedSwitch.get());
    }

    public void setTrim(double trim){
        this.trim = trim;
    }

    public double getTrim(){
        return trim;
    }
}
